/*----------------------------------------------------------------------------*/
/* Team 3647
/* DriveBase.java
/* Has all driving functions and bridge lowering
/*----------------------------------------------------------------------------*/
package Robot;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Joystick;

public class DriveTrain {
    
    public static final  Joystick GamePad = new Joystick(1);
    public static final  Joystick OpStick = new Joystick(2);
    
    private static final Compressor comp = new Compressor(1,1);
    private static final LinearVictor victorRight = new LinearVictor(1);
    private static final LinearVictor victorLeft  = new LinearVictor(2);
    
    public static void compressor() {
        if(OpStick.getRawButton(6)) {
            comp.stop();
        }
        else if(OpStick.getRawButton(7)) {
        comp.start();
        }
    }
    
    /***************************************************************************
     * Drive function based off two joysticks
     * Uses the Cheesy Drive function.
     ***************************************************************************/
    private static boolean quickTurn = true;
    public static void TankCD() {
        double LeftJoyStick  = GamePad.getRawAxis(2);
        double RightJoyStick = GamePad.getRawAxis(4);
        
//        if ((LeftJoyStick*RightJoyStick)>-.68 && Math.abs(LeftJoyStick)+Math.abs(RightJoyStick)>1.8)       
//            quickTurn = true;        
//        else
//            quickTurn = false;
        
        double throttle = (LeftJoyStick + RightJoyStick) / 2;
        double wheel    = (LeftJoyStick - RightJoyStick) / 2;
        
        if (GamePad.getRawButton(8)) 
            throttle = -.8;
        else if (GamePad.getRawButton(7)) 
            throttle =  .8; 
           
        if (Math.abs(throttle) <= 0.3 && Math.abs(throttle) >= 0.05) 
            throttle = 0.3 * (throttle < 0 ? -1 : 1);
        else if (Math.abs(throttle) < 0.05) 
            throttle = 0;
        
        CheesyDrive(throttle, wheel, quickTurn);
    }
    
    private static void CheesyDrive(double throttle, double wheel, boolean quickTurn) {
        double angular_power = 0.0;
        double overPower = 0.0;
        double sensitivity = 1.0;
        double rPower = 0.0;
        double lPower = 0.0;
        if (quickTurn) {
            overPower = .25;
            sensitivity = .75;
            angular_power = wheel;
        } else {
            overPower = 0.0;
            angular_power = Math.abs(throttle) * wheel * sensitivity;
        }
        rPower = lPower = throttle;
        lPower += angular_power;
        rPower -= angular_power;
        if (lPower > 1.0) {
            rPower -= overPower * (lPower - 1.0);
            lPower = 1.0;
        } else if (rPower > 1.0) {
            lPower -= overPower * (rPower - 1.0);
            rPower = 1.0;
        } else if (lPower < -1.0) {
            rPower += overPower * (-1.0 - lPower);
            lPower = -1.0;
        } else if (rPower < -1.0) {
            lPower += overPower * (-1.0 - rPower);
            rPower = -1.0;
        }
        Drive(lPower, rPower);
    }

    /***************************************************************************
     * Drive function
     * Sets each side of drive motor
     ***************************************************************************/
    private static void Drive(double left, double right) {
        victorLeft.set(-left);
        victorRight.set(right);
    }
    
    public static void CompStart() {
        comp.start();
    }
}
